Rapid Loop Updates

An important component of any simultaneous localization and mapping (SLAM) system, as well as modern vision-aided navigation systems, is the ability to efficiently incorporate loop-closure measurements. EKF-SLAM and full-SLAM are, arguably, the two common approaches that have been developed over the years. The former maintains the observed landmarks and the current pose while marginalizing out past robot poses. The latter, maintains both the observed landmarks and all robot’s poses past poses and current pose. Fusing loop-closure measurements both in EKF-SLAM and full-SLAM typically involves considerable computations causing to non-negligible delays, depending on the number of involved variables. Since many robotic applications require a navigation solution at a high frequency, it was proposed [7, 6] to incorporate loop closure measurements in a background process while fusing any other measurements in a foreground high-rate process. Thus, in [7] an EKF is used for processing incoming IMU and visual measurements, and bundle adjustment (BA) is applied in a background process whenever a loop-closure measurement is obtained, or from time to time for limiting linearization errors. More recently, the authors of [6] show how these two processes (filtering and smoothing) can be parallelized into a single optimization problem, which was not addressed previously. Incorporating loop-closure measurements can thus be performed while still operating at high-frequency [6]. However, the navigation solution calculated by the filter is not updated with loop-closure information until the smoother has finished processing the loop-closure measurement in a background process. Thus, during that time the solution calculated by the filter does not utilize all the available measurements and thus is sub-optimal. Rapid loop updates (RLU) is a suggested approach to address this issue. Upon obtaining a loop closure measurement, the idea is to update the filter with an approximate loop-closure update, while the smoother is engaged in calculating the exact update. Calculating this approximate update is expected to be computationally cheap and fast, since it only involves variables that are related by the loop-closure measurement. This is in contrast to calculating the exact update by the smoother, which typically requires optimizing all the